#include "estimator.h"

Estimator::Estimator(): f_manager{Rs}{
    LOG(INFO) << "init begins" << std::endl;
    clearState();
}

void Estimator::setParameter(){
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRbc(ric[0]);
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

void Estimator::clearState(){
    for (int i = 0; i < WINDOW_SIZE + 1; i++){
        Rs[i].setIdentity();
        Ps[i].setZero();
        Vs[i].setZero();
        Bas[i].setZero();
        Bgs[i].setZero();
        dt_buf[i].clear();
        linear_acceleration_buf[i].clear();
        angular_velocity_buf[i].clear();

        if (pre_integrations[i] != nullptr)
            delete pre_integrations[i];
        pre_integrations[i] = nullptr;
    }

    for (int i = 0; i < NUM_OF_CAM; i++){
        tic[i] = Vector3d::Zero();
        ric[i] = Matrix3d::Identity();
    }

    for (auto &it : all_image_frame){
        if (it.second.pre_integration != nullptr){
            delete it.second.pre_integration;
            it.second.pre_integration = nullptr;
        }
    }

    solver_flag = INITIAL;
    first_imu = false,
    sum_of_back = 0;
    sum_of_front = 0;
    frame_count = 0;
    solver_flag = INITIAL;
    initial_timestamp = 0;
    all_image_frame.clear();
    td = TD;


    if (tmp_pre_integration != nullptr)
        delete tmp_pre_integration;
//    if (last_marginalization_info != nullptr)
//        delete last_marginalization_info;

    tmp_pre_integration = nullptr;
//    last_marginalization_info = nullptr;
    last_marginalization_parameter_blocks.clear();

    f_manager.clearState();

    failure_occur = 0;
    relocalization_info = 0;

    drift_correct_r = Matrix3d::Identity();
    drift_correct_t = Vector3d::Zero();
}

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity){

    if (!first_imu){
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }

    if (!pre_integrations[frame_count]){
        pre_integrations[frame_count] = new IMUpreintegration{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
    if (frame_count != 0){
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)
            tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        int j = frame_count;         
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        Rs[j] = Rs[j] * Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        Ps[j] = Ps[j] + dt * Vs[j] + 0.5 * dt * dt * un_acc;
        Vs[j] = Vs[j] + dt * un_acc;
    }
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header){

    LOG(INFO) << "new image coming ------------------------------------------" << std::endl;
    LOG(INFO) << "Adding feature points " << image.size() << std::endl;
    if (f_manager.addFeatureAndCheckParallax(frame_count, image)){
        marginalization_flag = MARGIN_OLD;
        LOG(INFO) <<  "MARGIN_OLD" << std::endl;
    }
    else{
        marginalization_flag = MARGIN_SECOND_NEW;
        LOG(INFO) <<  "MARGIN_SECOND_NEW" << std::endl;
    }

    LOG(INFO) << "Solving " <<  frame_count << std::endl;
    LOG(INFO) << "number of feature: " << f_manager.getFeatureCount() << std::endl;
    Headers[frame_count] = header;

    ImageFrame imageframe(image, header.stamp.toSec());
//    if(tmp_pre_integration){
//        std::cout << "jacobian: " << tmp_pre_integration->jacobian.block<3,3>(3, 12) << std::endl;
//    }
    imageframe.pre_integration = tmp_pre_integration;
    all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));
    tmp_pre_integration = new IMUpreintegration{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

    if(ESTIMATE_EXTRINSIC == 2){
        LOG(INFO) << "calibrating extrinsic param, rotation movement is needed" << std::endl;
        if (frame_count != 0){
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
//            for(int i = 0; i < corres.size(); i++){
//                std::cout << "corres[i].first: " << corres[i].first.transpose()
//                          << " corres[i].second: " << corres[i].second.transpose() << std::endl;
//            }

            Matrix3d calib_ric;
            if (initial_ex_rotation.CalibrationExRbc(corres, pre_integrations[frame_count]->deltaQ.toRotationMatrix(), calib_ric)){
                LOG(INFO) << "initial extrinsic rotation calib success" << std::endl;
                LOG(INFO) << "initial extrinsic rotation: " << endl << Utility::R2ypr(calib_ric).transpose() << std::endl;
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }

    if (solver_flag == INITIAL){
        if (frame_count == WINDOW_SIZE){
            bool result = false;
            if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1){
               result = initialStructure();
               initial_timestamp = header.stamp.toSec();
            }
            if(result){
                solver_flag = NON_LINEAR;
                solveOdometry();
                slideWindow();
                f_manager.removeFailures();
                LOG(INFO) << "Initialization finish!" << std::endl;
                last_R = Rs[WINDOW_SIZE];
                last_P = Ps[WINDOW_SIZE];
                last_R0 = Rs[0];
                last_P0 = Ps[0];
            }
            else{
                slideWindow();
            }
        }
        else{
            frame_count++;
        }
    }
    else{
        TicToc t_solve;
        solveOdometry();
        LOG(INFO) << "solver costs: " << t_solve.toc() << std::endl;

        if (failureDetection()){
            LOG(WARNING) << "failure detection!" << std::endl;
            failure_occur = 1;
            clearState();
            setParameter();
            LOG(WARNING) << "system reboot!" << std::endl;
            return;
        }

        TicToc t_margin;
        slideWindow();
        f_manager.removeFailures();
        LOG(INFO) << "marginalization costs: " << t_margin.toc() << std::endl;

//        key_poses.clear();
//        for (int i = 0; i <= WINDOW_SIZE; i++)
//            key_poses.push_back(Ps[i]);

        last_R = Rs[WINDOW_SIZE];
        last_P = Ps[WINDOW_SIZE];
        last_R0 = Rs[0];
        last_P0 = Ps[0];
    }
}

bool Estimator::computeRelativeR(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>& corres, Eigen::Matrix3d& R, Eigen::Vector3d& t){
    if(corres.size() >= 15){
        std::vector<cv::Point2f> ll, rr;
        for(int i = 0; i < corres.size(); i++){
            ll.push_back(cv::Point2f(corres[i].first.x(), corres[i].first.y()));
            rr.push_back(cv::Point2f(corres[i].second.x(), corres[i].second.y()));
        }
        cv::Mat mask;
        cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
        cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
        cv::Mat tempR, tempt, r;
        int inliers = recoverPose(E, ll, rr, cameraMatrix, tempR, tempt, mask);
        LOG(INFO) << "inliers: " << inliers << std::endl;
        Eigen::Matrix3d R1;
        Eigen::Vector3d t1;
//        LOG(INFO) << "tempR: " << tempR << std::endl;
//        cv::Rodrigues(tempR, r);
//        LOG(INFO) << "r: " << r << std::endl;
        cv::cv2eigen(tempR, R1);
//        LOG(INFO) << "R1: " << R1 << std::endl;
        cv::cv2eigen(tempt, t1);
//        LOG(INFO) << "t1: " << t1 << std::endl;
        //note;
        t = - R1.transpose() * t1;
        R = R1.transpose();
        if(inliers > 12){
            return true;
        }else{
            return false;
        }

    }
    return false;
}

bool Estimator::relativeRT(Eigen::Matrix3d &relativeR, Eigen::Vector3d &relativeT, int &l) {

    for(int i = 0; i < WINDOW_SIZE; i++){
        auto corres = f_manager.getCorresponding(i, WINDOW_SIZE);
        if(corres.size() > 20){
            double sum_parallax = 0.0;
            for(int j = 0; j < corres.size(); j++){
                Eigen::Vector2d pt_i(corres[j].first(0), corres[j].first(1));
                Eigen::Vector2d pt_j(corres[j].second(0), corres[j].second(1));
                //double temp_parallax = std::sqrt(std::pow(pt_i.x() - pt_j.x(), 2) + std::pow(pt_i.y() - pt_j.y(), 2));
                double temp_parallax = (pt_i - pt_j).norm();
                sum_parallax += temp_parallax;
            }
            double average_parallax = 1.0 * sum_parallax / (corres.size());
            if(average_parallax * 460 > 30 && computeRelativeR(corres, relativeR, relativeT)){
                l = i;
                LOG(INFO) << "average_parallax * 460: " << average_parallax * 460 << std::endl;
                LOG(INFO) << " FIND L IS OK!! L is " << l << std::endl;
                return true;
            }
        }
    }
    return false;
}

bool Estimator::initialStructure() {
    //check imu
    {
        std::map<double, ImageFrame>::iterator it;
        //
        Eigen::Vector3d sum_g = Eigen::Vector3d::Zero();
        for( it = all_image_frame.begin(), it++; it != all_image_frame.end(); it++){
            double dt = it->second.pre_integration->sum_dt;
            Eigen::Vector3d temp_g = it->second.pre_integration->deltaV / dt;
            sum_g += temp_g;
        }
        Eigen::Vector3d avg_g = sum_g / (1.0 * (all_image_frame.size() - 1));
        double var = 0.0;
        for( it = all_image_frame.begin(), it++; it != all_image_frame.end(); it++){
            double dt = it->second.pre_integration->sum_dt;
            Eigen::Vector3d temp_g = it->second.pre_integration->deltaV / dt;
            var += (temp_g - avg_g).transpose() * (temp_g - avg_g);
        }
        var = std::sqrt(var / (all_image_frame.size() - 1));

        if(var < 0.25){
            LOG(INFO) << "imu excitation not enough!!!" << std::endl;
        }
    }
    Eigen::Quaterniond Q[frame_count + 1];
    Eigen::Vector3d T[frame_count + 1];
    std::vector<SfmFeature> sfmVec;
    for(auto& it : f_manager.allFeatures){
        SfmFeature temp;
        temp.state = false;
        temp.id = it.featurId;
        int j = it.start_frame - 1;
        for(auto& item : it.features_per_id){
            j++;
            temp.observation.push_back(std::make_pair(j, Eigen::Vector2d(item.point.x(), item.point.y())));
        }
        sfmVec.push_back(temp);
    }

    Eigen::Matrix3d relativeR;
    Eigen::Vector3d relativeT;
    int l;
    if(!relativeRT(relativeR, relativeT, l)){
        return false;
    }
    LOG(INFO) << "relativeR: " << relativeR << " relativeT: " << relativeT << std::endl;
    LOG(INFO) << "relativeR_ypr: " << Utility::R2ypr(relativeR).transpose() << std::endl;
    GlobalSfm globalSfm;
    std::map<int, Eigen::Vector3d> sfm_tracked_points;
    if(!globalSfm.construct(frame_count + 1, Q, T, l, relativeR, relativeT, sfmVec, sfm_tracked_points)){
        LOG(INFO) << "SFM Faild" << std::endl;
        marginalization_flag = MarginalizationFlag::MARGIN_OLD;
        return false;
    }
//    for(int i = 0; i < frame_count + 1; i++){
//        LOG(INFO) << i << "  |  " << Q[i] << "  |  " <<  T[i].transpose() << std::endl;
//    }
//    for(int i = 0; i < sfmVec.size(); i++){
//        LOG(INFO) << i << "  |  " << sfmVec[i].position[0] << " "
//                                    << sfmVec[i].position[1] << " "
//                                    << sfmVec[i].position[2] << std::endl;
//    }
    LOG(INFO) << "SFM SUCESS!!" << std::endl;

    auto frame_it = all_image_frame.begin();
    for(int i = 0; frame_it != all_image_frame.end(); frame_it++){
        if(frame_it->first == Headers[i].stamp.toSec()){
            frame_it->second.is_key_frame = true;
            frame_it->second.R = Q[i].toRotationMatrix() * ric[0].transpose();
            frame_it->second.T = T[i];

            i++;
            continue;
        }

        if(frame_it->first > Headers[i].stamp.toSec()){
            i++;
        }
        frame_it->second.is_key_frame = false;

        Eigen::Matrix3d R_initial = Q[i].inverse().toRotationMatrix();
        Eigen::Vector3d T_initial = -Q[i].inverse().toRotationMatrix() * T[i];

        std::vector<cv::Point3f> points_3d;
        std::vector<cv::Point2f> points_2d;
        for(auto& pts : frame_it->second.points){
            int curId = pts.first;
            for(auto& p : pts.second){
                auto iter = sfm_tracked_points.find(curId);
                if(iter != sfm_tracked_points.end()){
                    cv::Point3f  wP3(iter->second.x(), iter->second.y(), iter->second.z());
                    points_3d.push_back(wP3);
                    cv::Point2f  cP2(p.second[0], p.second[1]);
                    points_2d.push_back(cP2);
                }
            }
        }
        cv::Mat K = cv::Mat::eye(3, 3, CV_64F);
        cv::Mat D, revc, t, tempR;
        cv::eigen2cv(R_initial, tempR);
        cv::Rodrigues(tempR, revc);
        cv::eigen2cv(T_initial, t);
        if(points_3d.size() < 6){
            LOG(INFO) << "pts_3_vector size " << points_3d.size() << std::endl;
            LOG(INFO) << "Not enough points for solve pnp !" << std::endl;
            return false;
        }

        if(!cv::solvePnP(points_3d, points_2d, K, D, revc, t, 1)){
            LOG(INFO) << "solvePnP fail!" << std::endl;
            return false;
        }

        cv::Rodrigues(revc, tempR);
        cv::cv2eigen(tempR, R_initial);
        cv::cv2eigen(t, T_initial);

//        R_initial = R_initial.transpose();
//        T_initial = -R_initial * T_initial;

        frame_it->second.R = R_initial.transpose() * ric[0].transpose();
        frame_it->second.T = -R_initial.transpose() * T_initial;

    }

//    for(auto kk = all_image_frame.begin(); kk != all_image_frame.end(); kk++){
//        LOG(INFO) << "kk->second.R: " << kk->second.R << "   t:  " << kk->second.t << std::endl;
//    }

    // 下面是视觉+IMU对齐部分。
    // 1. Bais的估计
    solveGyrBais(all_image_frame, Bgs);
    //2. 视觉+ IMU的预积分对齐 以及 refine g
    Eigen::VectorXd x;
    if(!preAlignment(all_image_frame, g, x)){
        return false;
    }
    LOG(INFO) << "g.norm: " << g.norm() << " g.transpose(): " << g.transpose() << std::endl;

//    for(auto kk = all_image_frame.begin(); kk != all_image_frame.end(); kk++){
//        LOG(INFO) << "kk->second.R: " << Eigen::Quaterniond(kk->second.R) << "   t:  " << kk->second.t.transpose() << std::endl;
//    }

    //change state
    for(int i = 0; i <= frame_count; i++){
        double timeIndex = Headers[i].stamp.toSec();
        Ps[i] = all_image_frame[timeIndex].T;
        Rs[i] = all_image_frame[timeIndex].R;
        all_image_frame[timeIndex].is_key_frame = true;
    }
    Eigen::VectorXd depthVec = f_manager.getDepthVector();
    for (int i = 0; i < depthVec.size(); i++)
        depthVec[i] = -1;
    f_manager.clearDepth(depthVec);

    f_manager.setRbc(ric[0]);
    f_manager.triangulation(Ps, tic[0], ric[0]);

    double s = (x.tail<1>())(0);
    for(int i = 0; i <= WINDOW_SIZE; i++){
        pre_integrations[i]->repropagate(Eigen::Vector3d::Zero(), Bgs[i]);
    }
    for(int i = frame_count; i >= 0; i--){
        Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0] );
    }
    int v = -1;
    int m = 0;
//    LOG(INFO) << "x: " << x.rows() << std::endl;
    std::map<double, ImageFrame>::iterator frame_i;
    for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
    {
        if(frame_i->second.is_key_frame)
        {
            v++;
            Vs[v] = frame_i->second.R * x.segment<3>(v * 3);
        }
        m++;
    }

    for(auto& it_per_ids : f_manager.allFeatures){
        it_per_ids.used_num = it_per_ids.features_per_id.size();
        if (!(it_per_ids.used_num >= 2 && it_per_ids.start_frame < WINDOW_SIZE - 2))
            continue;
        it_per_ids.estimated_depth *= s;
        LOG(INFO) << "it_per_ids.estimated_depth: " << it_per_ids.estimated_depth << std::endl;
    }

    Eigen::Matrix3d R0  = Utility::g2R(g);
    double yaw = Utility::R2ypr(R0 * Rs[0]).x();
    R0 = Utility::ypr2R(Eigen::Vector3d(-yaw, 0, 0)) * R0;
    g = R0 * g;
    for(int i = 0; i <= frame_count ; i++){
        Ps[i] = R0 * Ps[i];
        Rs[i] = R0 * Rs[i];
        Vs[i] = R0 * Vs[i];
    }
    LOG(INFO) << "g0: " << g.transpose() << std::endl;
    LOG(INFO) << "Utility::R2ypr(Rs[0])): " << Utility::R2ypr(Rs[0]).transpose() << std::endl;

//    for (int i = 0; i <= frame_count; i++)
//    {
//        LOG(INFO) << " Ps: " << i << " " << Ps[i].transpose() << std::endl;
//        LOG(INFO) << " Rs: " << i << " " << Eigen::Quaterniond(Rs[i]) << std::endl;
//        LOG(INFO) << " Vs: " << i << " " << Vs[i].transpose() << std::endl;
//    }

    return true;
}

void Estimator::solveOdometry()
{
    if (frame_count < WINDOW_SIZE)
        return;
    if (solver_flag == NON_LINEAR)
    {
        TicToc t_tri;
        f_manager.triangulation(Ps, tic[0], ric[0]);
        ROS_DEBUG("triangulation costs %f", t_tri.toc());
        optimization();
    }
}

bool Estimator::failureDetection(){

    if (Bas[WINDOW_SIZE].norm() > 2.5){
        LOG(INFO) << "big IMU acc bias estimation: " << Bas[WINDOW_SIZE].norm() << std::endl;
        return true;
    }
    if (Bgs[WINDOW_SIZE].norm() > 1.0){
        LOG(INFO) << "big IMU gyr bias estimation: " << Bgs[WINDOW_SIZE].norm() << std::endl;
        return true;
    }

    Vector3d tmp_P = Ps[WINDOW_SIZE];
    if ((tmp_P - last_P).norm() > 5){
        LOG(INFO) << " big translation" << std::endl;
        return true;
    }
    if (abs(tmp_P.z() - last_P.z()) > 1){
        LOG(INFO) << " big z translation" << std::endl;
        return true;
    }
    Matrix3d tmp_R = Rs[WINDOW_SIZE];
    Matrix3d delta_R = tmp_R.transpose() * last_R;
    Quaterniond delta_Q(delta_R);
    double delta_angle;
    delta_angle = acos(delta_Q.w()) * 2.0 / 3.14 * 180.0;
    if (delta_angle > 50){
        LOG(INFO) << " big delta_angle " << std::endl;
        //return true;
    }
    return false;
}

void Estimator::slideWindow(){

    if(marginalization_flag == MARGIN_OLD){
        double t0 = Headers[0].stamp.toSec();
        back_P0 = Ps[0];
        back_R0 = Rs[0];
        if(frame_count == WINDOW_SIZE){
            for(int i = 0; i < WINDOW_SIZE; i++){
                Headers[i] = Headers[i + 1];
                Ps[i] = Ps[i + 1];
                Rs[i] = Rs[i + 1];
                Vs[i] = Vs[i + 1];
                Bas[i] = Bas[i + 1];
                Bgs[i] = Bgs[i + 1];
                std::swap(pre_integrations[i] , pre_integrations[i + 1]);

                dt_buf[i].swap(dt_buf[i + 1]);
                linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
                angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);
            }
            Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
            Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
            Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
            Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
            Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
            Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];

            delete pre_integrations[WINDOW_SIZE];
            pre_integrations[WINDOW_SIZE] = new IMUpreintegration(acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]);

            dt_buf[WINDOW_SIZE].clear();
            linear_acceleration_buf[WINDOW_SIZE].clear();
            angular_velocity_buf[WINDOW_SIZE].clear();

            if(true || solver_flag == SolverFlag::INITIAL){
                std::map<double, ImageFrame>::iterator it1;
                it1 = all_image_frame.find(t0);
                delete it1->second.pre_integration;
                it1->second.pre_integration = nullptr;
                for(std::map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it1; it++){
                    if(it->second.pre_integration){
                        delete it->second.pre_integration;
                    }
                    it->second.pre_integration = nullptr;
                }
                all_image_frame.erase(all_image_frame.begin(), it1);
                all_image_frame.erase(it1);
            }
            bool shift_depth = solver_flag == SolverFlag::NON_LINEAR ? true : false;
            if(shift_depth){

                Eigen::Matrix3d R0, R1;
                Eigen::Vector3d T0, T1;
                R0 = back_R0 * ric[0];
                T0 = back_R0 * tic[0] + back_P0;

                R1 = Rs[0] * ric[0];
                T0 = Rs[0] * tic[0] + Ps[0];

                f_manager.removeOldAndShiftDepth(R0, T0, R1, T1);
            }
            else{
                f_manager.removeOld();
            }
        }
    }
    else{
        if(frame_count == WINDOW_SIZE){
            for(int i = 0; i < dt_buf[frame_count].size(); i++){

                double temp_t = dt_buf[frame_count][i];
                Eigen::Vector3d temp_acc = linear_acceleration_buf[frame_count][i];
                Eigen::Vector3d temp_gyr = angular_velocity_buf[frame_count][i];

                pre_integrations[frame_count - 1]->push_back(temp_t, temp_acc, temp_gyr);

                dt_buf[frame_count - 1].push_back(temp_t);
                linear_acceleration_buf[frame_count - 1].push_back(temp_acc);
                angular_velocity_buf[frame_count - 1].push_back(temp_gyr);
            }
            Headers[frame_count - 1] = Headers[frame_count];
            Ps[frame_count - 1] = Ps[frame_count];
            Rs[frame_count - 1] = Rs[frame_count];
            Vs[frame_count - 1] = Vs[frame_count];
            Bas[frame_count - 1] = Bas[frame_count];
            Bgs[frame_count - 1] = Bgs[frame_count];

            delete pre_integrations[WINDOW_SIZE];
            pre_integrations[WINDOW_SIZE] = new IMUpreintegration(acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]);

            dt_buf[WINDOW_SIZE].clear();
            linear_acceleration_buf[WINDOW_SIZE].clear();
            angular_velocity_buf[WINDOW_SIZE].clear();
            f_manager.removeSecondNew(frame_count);
        }
    }
}

void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r){
    relo_frame_stamp = _frame_stamp;
    relo_frame_index = _frame_index;
    match_points.clear();
    match_points = _match_points;
    prev_relo_t = _relo_t;
    prev_relo_r = _relo_r;
    for(int i = 0; i < WINDOW_SIZE; i++){
        if(relo_frame_stamp == Headers[i].stamp.toSec()){
            relo_frame_local_index = i;
            relocalization_info = 1;
            for (int j = 0; j < SIZE_POSE; j++)
                relo_Pose[j] = para_Pose[i][j];
        }
    }
}

void Estimator::optimization(){

    ceres::Problem problem;
    ceres::LossFunction *loss_function;
    //loss_function = new ceres::HuberLoss(1.0);
    loss_function = new ceres::CauchyLoss(1.0);
    for (int i = 0; i < WINDOW_SIZE + 1; i++)
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Pose[i], 7, local_parameterization);
        problem.AddParameterBlock(para_SpeedBias[i], 9);
    }
    for (int i = 0; i < 1; i++)
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Ex_Pose[i], 7, local_parameterization);
    }

    vector2double();

    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        int j = i + 1;
        if (pre_integrations[j]->sum_dt > 10.0)
            continue;
        IMUFaxtor* imu_factor = new IMUFaxtor(pre_integrations[j]);
        problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
    }
    int f_m_cnt = 0;
    int feature_index = -1;
    for (auto &it_per_id : f_manager.allFeatures)
    {
        it_per_id.used_num = it_per_id.features_per_id.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;

        ++feature_index;

        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;

        Vector3d pts_i = it_per_id.features_per_id[0].point;

        for (auto &it_per_frame : it_per_id.features_per_id)
        {
            imu_j++;
            if (imu_i == imu_j)
            {
                continue;
            }
            Vector3d pts_j = it_per_frame.point;

            ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
            problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);

            f_m_cnt++;
        }
    }

    if(relocalization_info){
        ceres::LocalParameterization* localParameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(relo_Pose, 7, localParameterization);

        int feature_index = -1;
        int index = 0;
        for(auto & it_per_id : f_manager.allFeatures){
            it_per_id.used_num = it_per_id.features_per_id.size();
            if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)){
                continue;
            }
            ++feature_index;
            int imu_i = it_per_id.start_frame;
            if(imu_i <= relo_frame_local_index){
                while(match_points[index].z() < it_per_id.featurId){
                    index++;
                }
                if(match_points[index].z() == it_per_id.featurId){
                    Eigen::Vector3d pts_j = Vector3d(match_points[index].x(), match_points[index].y(), 1.0);
                    Eigen::Vector3d pts_i = it_per_id.features_per_id[0].point;

                    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                    problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
                    index++;
                }
            }
        }
    }

    ceres::Solver::Options options;

    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.trust_region_strategy_type = ceres::DOGLEG;
    options.max_num_iterations = 8;
    if (marginalization_flag == MARGIN_OLD)
        options.max_solver_time_in_seconds = 0.04 * 4.0 / 5.0;
    else
        options.max_solver_time_in_seconds = 0.04;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    LOG(INFO) << summary.BriefReport() << std::endl;

    double2vector();
//    LOG(INFO) << "1111111111111111" << std::endl;
    if(marginalization_flag == MARGIN_OLD){
        MarginalizationInfo* marginalizationInfo = new MarginalizationInfo();
        vector2double();
//        LOG(INFO) << "1111111111111111" << std::endl;
        if(lastMarginalizationInfo){
            std::vector<int> drop_set;
            for(int i = 0; i < last_marginalization_parameter_blocks.size(); i++){
                if(last_marginalization_parameter_blocks[i] == para_Pose[0]
                 || last_marginalization_parameter_blocks[i] == para_SpeedBias[0]){
                    drop_set.push_back(i);
                }
            }
            MarginalizationFactor* last_marginalizationFactor = new MarginalizationFactor(lastMarginalizationInfo);
            ResidualBlockInfo* last_residualBlockInfo = new ResidualBlockInfo(last_marginalizationFactor, nullptr,
                                                                              last_marginalization_parameter_blocks,
                                                                              drop_set);
            marginalizationInfo->addResidualBlockInfo(last_residualBlockInfo);
        }
//        LOG(INFO) << "1111111111111111" << std::endl;
        {
            //处理预积分
            if(pre_integrations[1]->sum_dt < 10.0){
                IMUFaxtor* imuFaxtor = new IMUFaxtor(pre_integrations[1]);
                ResidualBlockInfo* imu_residualBlockInfo = new ResidualBlockInfo(imuFaxtor, nullptr,
                                     std::vector<double*>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
                                     std::vector<int>{0, 1});
                marginalizationInfo->addResidualBlockInfo(imu_residualBlockInfo);
            }
        }
//        LOG(INFO) << "1111111111111111" << std::endl;
        {
            //处理视觉特征点
            int feature_index = -1;
            for(auto& it : f_manager.allFeatures){
                it.used_num = it.features_per_id.size();
                if(!(it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2)){
                    continue;
                }
                ++feature_index;
                int imu_i = it.start_frame;
                int imu_j = imu_i - 1;
                if(imu_i != 0){
                    continue;
                }

                Eigen::Vector3d pts_i = it.features_per_id[0].point;
                for (auto &it_per_frame : it.features_per_id){
                    imu_j++;
                    if (imu_i == imu_j){
                        continue;
                    }
                    Vector3d pts_j = it_per_frame.point;
                    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                    ResidualBlockInfo* feature_residualBlockInfo = new ResidualBlockInfo(f, loss_function,
                                       std::vector<double*>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
                                       std::vector<int>{0, 3});

                    marginalizationInfo->addResidualBlockInfo(feature_residualBlockInfo);
                }
            }
        }
//        LOG(INFO) << "1111111111111111" << std::endl;
        marginalizationInfo->preMarginaliza();
//        LOG(INFO) << "1111111111111111" << std::endl;
        marginalizationInfo->marginalize();
//        LOG(INFO) << "1111111111111111" << std::endl;
        std::unordered_map<long, double*> addr_shift;
        for(int i = 1; i <= WINDOW_SIZE; i++){
            addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
            addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
        }
        for(int i = 0; i < NUM_OF_CAM; i++){
            addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
        }
        std::vector<double*> parameters_blocks;
        parameters_blocks = marginalizationInfo->getParameterBlocks(addr_shift);
        if(lastMarginalizationInfo){
            delete lastMarginalizationInfo;
        }
        lastMarginalizationInfo = marginalizationInfo;
        last_marginalization_parameter_blocks = parameters_blocks;
    }
    else{
//        LOG(INFO) << "1111111111111111" << std::endl;
        if(lastMarginalizationInfo
        && std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1])){
            MarginalizationInfo* marginalizationInfo = new MarginalizationInfo();
            vector2double();
            if(lastMarginalizationInfo){
//                LOG(INFO) << "1111111111111111" << std::endl;
                std::vector<int> drop_set;
                for(int i = 0; i < last_marginalization_parameter_blocks.size(); i++){
                    if(last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1]){
                        drop_set.push_back(i);
                    }
                }
                MarginalizationFactor* marginalizationFactor = new MarginalizationFactor(lastMarginalizationInfo);
                ResidualBlockInfo* last_residualBlockInfo = new ResidualBlockInfo(marginalizationFactor, nullptr,
                                                                                  last_marginalization_parameter_blocks,
                                                                                  drop_set);
                marginalizationInfo->addResidualBlockInfo(last_residualBlockInfo);
            }

            marginalizationInfo->preMarginaliza();
            marginalizationInfo->marginalize();

            std::unordered_map<long, double*> addr_shift;
            for(int i = 0; i <= WINDOW_SIZE; i++){
                if(i == WINDOW_SIZE - 1){
                    continue;
                }
                else if(i == WINDOW_SIZE){
                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
                }
                else{
                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
                }
            }
            for(int i = 0; i < NUM_OF_CAM; i++){
                addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
            }
            std::vector<double*> parameters_blocks = marginalizationInfo->getParameterBlocks(addr_shift);
            if(lastMarginalizationInfo){
                delete lastMarginalizationInfo;
            }
            lastMarginalizationInfo = marginalizationInfo;
            last_marginalization_parameter_blocks = parameters_blocks;
        }
    }
}

void Estimator::vector2double() {
    for(int i = 0; i <= WINDOW_SIZE; i++){
        para_Pose[i][0] = Ps[i].x();
        para_Pose[i][1] = Ps[i].y();
        para_Pose[i][2] = Ps[i].z();
        Eigen::Quaterniond q(Rs[i]);
        para_Pose[i][3] = q.x();
        para_Pose[i][4] = q.y();
        para_Pose[i][5] = q.z();
        para_Pose[i][6] = q.w();

        para_SpeedBias[i][0] = Vs[i].x();
        para_SpeedBias[i][1] = Vs[i].y();
        para_SpeedBias[i][2] = Vs[i].z();

        para_SpeedBias[i][3] = Bas[i].x();
        para_SpeedBias[i][4] = Bas[i].y();
        para_SpeedBias[i][5] = Bas[i].z();

        para_SpeedBias[i][6] = Bgs[i].x();
        para_SpeedBias[i][7] = Bgs[i].y();
        para_SpeedBias[i][8] = Bgs[i].z();
    }
    Eigen::Quaterniond q(ric[0]);
    para_Ex_Pose[0][0] = tic[0].x();
    para_Ex_Pose[0][1] = tic[0].y();
    para_Ex_Pose[0][2] = tic[0].z();
    para_Ex_Pose[0][3] = q.x();
    para_Ex_Pose[0][4] = q.y();
    para_Ex_Pose[0][5] = q.z();
    para_Ex_Pose[0][6] = q.w();

    Eigen::VectorXd depVec = f_manager.getDepthVector();
    for(int i = 0; i < f_manager.getFeatureCount(); i++){
        para_Feature[i][0] = depVec[i];
    }
}

void Estimator::double2vector() {
    Eigen::Vector3d origin_R0 = Utility::R2ypr(Rs[0]);
    Eigen::Vector3d origin_P0 = Ps[0];
    if(failure_occur){
        failure_occur = false;
        origin_R0 = Utility::R2ypr(last_R0);;
        origin_P0 = last_P0;
    }

    Eigen::Vector3d optimizer_R0 = Utility::R2ypr(Eigen::Quaterniond(para_Pose[0][6], para_Pose[0][3],
                                                                     para_Pose[0][4], para_Pose[0][5]).toRotationMatrix());
    double diff_yaw = origin_R0.x() - optimizer_R0.x();
    Eigen::Matrix3d diff_rot = Utility::ypr2R(Eigen::Vector3d(diff_yaw, 0, 0));

    if(std::abs(std::abs(origin_R0.y()) - 90) < 1.0 || std::abs(std::abs(optimizer_R0.y()) - 90) < 1.0){
        diff_rot = Rs[0] * Eigen::Quaterniond(para_Pose[0][6],
                                              para_Pose[0][3],
                                              para_Pose[0][4],
                                              para_Pose[0][5]).toRotationMatrix().transpose();
    }

    for(int i = 0; i <= WINDOW_SIZE; i++){
//        LOG(INFO) << "delta_Bas: " << (Vector3d(para_SpeedBias[i][3],
//                                                para_SpeedBias[i][4],
//                                                para_SpeedBias[i][5]) - Bas[i]).transpose() << std::endl;
//        LOG(INFO) << "delta_Bgs: " << (Vector3d(para_SpeedBias[i][6],
//                                                para_SpeedBias[i][7],
//                                                para_SpeedBias[i][8]) - Bgs[i]).transpose() << std::endl;
        //Ps[i] = Eigen::Vector3d(paraPose[i][0], paraPose[i][1], paraPose[i][2]);
        Ps[i] = diff_rot * Eigen::Vector3d (para_Pose[i][0] - para_Pose[0][0],
                                            para_Pose[i][1] - para_Pose[0][1],
                                            para_Pose[i][2] - para_Pose[0][2]) + origin_P0;
        Rs[i] = diff_rot * Eigen::Quaterniond (para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]);
        Vs[i] = diff_rot * Eigen::Vector3d(para_SpeedBias[i][0], para_SpeedBias[i][1], para_SpeedBias[i][2]);
        Bas[i] = Eigen::Vector3d(para_SpeedBias[i][3], para_SpeedBias[i][4], para_SpeedBias[i][5]);
        Bgs[i] = Eigen::Vector3d(para_SpeedBias[i][6], para_SpeedBias[i][7], para_SpeedBias[i][8]);

//        LOG(INFO) << "Ps: " << Ps[i].transpose() << std::endl;
//        LOG(INFO) << "Vs: " << Vs[i].transpose() << std::endl;
//        LOG(INFO) << "Rs: " << Eigen::Quaterniond(Rs[i]) << std::endl;
//        LOG(INFO) << "Bas: " << Bas[i].transpose() << std::endl;
//        LOG(INFO) << "Bgs: " << Bgs[i].transpose() << std::endl;
    }
    LOG(INFO) << "Ps: " << Ps[WINDOW_SIZE].transpose() << std::endl;
    LOG(INFO) << "Vs: " << Vs[WINDOW_SIZE].transpose() << std::endl;
    LOG(INFO) << "Rs: " << Eigen::Quaterniond(Rs[WINDOW_SIZE]) << std::endl;
    LOG(INFO) << "Bas: " << Bas[WINDOW_SIZE].transpose() << std::endl;
    LOG(INFO) << "Bgs: " << Bgs[WINDOW_SIZE].transpose() << std::endl;

    tic[0] = Eigen::Vector3d(para_Ex_Pose[0][0], para_Ex_Pose[0][1], para_Ex_Pose[0][2]);
    ric[0] = Eigen::Quaterniond (para_Ex_Pose[0][6], para_Ex_Pose[0][3], para_Ex_Pose[0][4], para_Ex_Pose[0][5]).toRotationMatrix();
//    LOG(INFO) << "t_bc: " << t_bc.transpose() << std::endl;
//    LOG(INFO) << "R_bc: " << Eigen::Quaterniond(R_bc) << std::endl;
    Eigen::VectorXd depVec = f_manager.getDepthVector();
    for(int i = 0; i < f_manager.getFeatureCount(); i++){
        depVec(i) = para_Feature[i][0];
    }
    f_manager.setDepthVector(depVec);

    if(relocalization_info){
        Eigen::Matrix3d reloR;
        Eigen::Vector3d reloT;
        reloR = diff_rot * Eigen::Quaterniond(relo_Pose[6], relo_Pose[3], relo_Pose[4], relo_Pose[5]).normalized().toRotationMatrix();
        reloT = diff_rot * Eigen::Vector3d (relo_Pose[0] - para_Pose[0][0],
                                            relo_Pose[1] - para_Pose[0][1],
                                            relo_Pose[2] - para_Pose[0][2]) + origin_P0;
        // relo_Pose优化的初始位姿是para_Pose[i][j]
        double drift_correct_yaw;
        drift_correct_yaw = Utility::R2ypr(prev_relo_r).x() - Utility::R2ypr(reloR).x();
        drift_correct_r = Utility::ypr2R(Vector3d(drift_correct_yaw, 0, 0));
        drift_correct_t = prev_relo_t - drift_correct_r * reloT;
        relo_relative_t = reloR.transpose() * (Ps[relo_frame_local_index] - reloT);
        relo_relative_q = reloR.transpose() * Rs[relo_frame_local_index];
        relo_relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs[relo_frame_local_index]).x() - Utility::R2ypr(reloR).x());
        relocalization_info = 0;
    }
}